/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "UPRE_Decision.h"
#include "UPRE_ROBOT_TASK.h"
#include "UPRE_GYRO.h"
#include "UPRE_LOCATION.h"
#include "vesc.h"
#include "UPRE_AirCylinder.h"
#include "Kalmanfilter.h"
#include "UPRE_MOVEBASE.h"
#include "UPRE_PATH.h"
#include "UPRE_PID.h"


/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
extern bool Pose_Update;

void Send_Gyro_Encoder(uint8_t h,
                       uint8_t c,
                       uint8_t l,
                       Pose *Gyro_Encoder,
                       uint8_t t)
{
static union {
    float Value;
    uint8_t P[4];

}Float2uchar;

    uint8_t USB_Tx_date[33];
    USB_Tx_date[0] = h;
    USB_Tx_date[1] = c;
    USB_Tx_date[2] = l;

    Float2uchar.Value = Chassis_Sense_Value.High_To_Goal_Top;
    USB_Tx_date[23] = Float2uchar.P[0];
    USB_Tx_date[24] = Float2uchar.P[1];
    USB_Tx_date[25] = Float2uchar.P[2];
    USB_Tx_date[26] = Float2uchar.P[3];
    Float2uchar.Value = Chassis_Sense_Value.High_To_Goal_Bottom;
    USB_Tx_date[27] = Float2uchar.P[0];
    USB_Tx_date[28] = Float2uchar.P[1];
    USB_Tx_date[29] = Float2uchar.P[2];
    USB_Tx_date[30] = Float2uchar.P[3];
    USB_Tx_date[31] = Flag_2_ICE;
    USB_Tx_date[32] = t;

    // 定义结构体数组指针指向结构体的地�?
    float *Gyro_Encoder_ptr = (float *)Gyro_Encoder;
    for (size_t i = 0; i < 5; ++i)
        memcpy(&USB_Tx_date[3 + 4 * i], &Gyro_Encoder_ptr[i], sizeof(float));
    for (size_t i = 0; i < sizeof(USB_Tx_date) / sizeof(USB_Tx_date[0]); i++) {
        HAL_UART_Transmit(&huart5,&USB_Tx_date[i],1,HAL_MAX_DELAY);
    }
}

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for MainTask */
osThreadId_t MainTaskHandle;
const osThreadAttr_t MainTask_attributes = {
  .name = "MainTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for ComunicateTask */
osThreadId_t ComunicateTaskHandle;
const osThreadAttr_t ComunicateTask_attributes = {
  .name = "ComunicateTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for MotroTask */
osThreadId_t MotroTaskHandle;
const osThreadAttr_t MotroTask_attributes = {
  .name = "MotroTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Location_Task */
osThreadId_t Location_TaskHandle;
const osThreadAttr_t Location_Task_attributes = {
  .name = "Location_Task",
  .stack_size = 2048 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Printf_Task */
osThreadId_t Printf_TaskHandle;
const osThreadAttr_t Printf_Task_attributes = {
  .name = "Printf_Task",
  .stack_size = 512 * 4,
  .priority = (osPriority_t) osPriorityBelowNormal,
};
/* Definitions for Path_Task */
osThreadId_t Path_TaskHandle;
const osThreadAttr_t Path_Task_attributes = {
  .name = "Path_Task",
  .stack_size = 10000 * 4,
  .priority = (osPriority_t) osPriorityBelowNormal,
};
/* Definitions for Auto_Find_Ball_ */
osThreadId_t Auto_Find_Ball_Handle;
const osThreadAttr_t Auto_Find_Ball__attributes = {
  .name = "Auto_Find_Ball_",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Robot_Move_Task */
osThreadId_t Robot_Move_TaskHandle;
const osThreadAttr_t Robot_Move_Task_attributes = {
  .name = "Robot_Move_Task",
  .stack_size = 2048 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartTask_Main(void *argument);
void StartTask_Comunicate(void *argument);
void StartTask_MotorControl(void *argument);
void Start_Location(void *argument);
void Start_Printf(void *argument);
void Start_Go_PAth(void *argument);
void Start_Find_Ball(void *argument);
void Start_Move(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of MainTask */
  MainTaskHandle = osThreadNew(StartTask_Main, NULL, &MainTask_attributes);

  /* creation of ComunicateTask */
  ComunicateTaskHandle = osThreadNew(StartTask_Comunicate, NULL, &ComunicateTask_attributes);

  /* creation of MotroTask */
  MotroTaskHandle = osThreadNew(StartTask_MotorControl, NULL, &MotroTask_attributes);

  /* creation of Location_Task */
  Location_TaskHandle = osThreadNew(Start_Location, NULL, &Location_Task_attributes);

  /* creation of Printf_Task */
  Printf_TaskHandle = osThreadNew(Start_Printf, NULL, &Printf_Task_attributes);

  /* creation of Path_Task */
  Path_TaskHandle = osThreadNew(Start_Go_PAth, NULL, &Path_Task_attributes);

  /* creation of Auto_Find_Ball_ */
  Auto_Find_Ball_Handle = osThreadNew(Start_Find_Ball, NULL, &Auto_Find_Ball__attributes);

  /* creation of Robot_Move_Task */
  Robot_Move_TaskHandle = osThreadNew(Start_Move, NULL, &Robot_Move_Task_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartTask_Main */
/**
  * @brief  Function implementing the MainTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartTask_Main */
void StartTask_Main(void *argument)
{
  /* USER CODE BEGIN StartTask_Main */
//    setAirCylinder_more(Air_2,Open_Air,8,OUT_1,OUT_2,OUT_3,OUT_4,OUT_5,OUT_6,OUT_7,OUT_8);
    /* Infinite loop */
  for(;;)
  {
      Time_Stamp();
      osDelay(1);
  }
  /* USER CODE END StartTask_Main */
}

/* USER CODE BEGIN Header_StartTask_Comunicate */
/**
* @brief Function implementing the ComunicateTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask_Comunicate */
void StartTask_Comunicate(void *argument)
{
  /* USER CODE BEGIN StartTask_Comunicate */
  /* Infinite loop */
  for(;;)
  {
      Send_Gyro_Encoder(0x55,0xaa,0x1d,&Robot_Pose,0x0d);
      osDelay(3);
  }
  /* USER CODE END StartTask_Comunicate */
}

/* USER CODE BEGIN Header_StartTask_MotorControl */
/**
* @brief Function implementing the MotroTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask_MotorControl */
void StartTask_MotorControl(void *argument)
{
  /* USER CODE BEGIN StartTask_MotorControl */
  /* Infinite loop */
  for(;;)
  {
      Task_Decision();
      Motor_Control(Motor_Set_param.receive_ball,Motor_Set_param.divide_ball,Motor_Set_param.trans_ball,Motor_Set_param.lay_ball);
      osDelay(5);

  }
  /* USER CODE END StartTask_MotorControl */
}

/* USER CODE BEGIN Header_Start_Location */
/**
* @brief Function implementing the Location_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Location */
void Start_Location(void *argument)
{
  /* USER CODE BEGIN Start_Location */
  /* Infinite loop */
  for(;;)
  {
      Encoder_On();
      cal_location();
      cal_BW_MINS500();

    osDelay(2);
  }
  /* USER CODE END Start_Location */
}

/* USER CODE BEGIN Header_Start_Printf */
/**
* @brief Function implementing the Printf_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Printf */
void Start_Printf(void *argument)
{
  /* USER CODE BEGIN Start_Printf */
    extern vector Print_Path_Vel;
    extern ChassisMovement Print_Pid_out;
    /* Infinite loop */
  for(;;)
  {
      vTaskSuspend(Printf_TaskHandle);
//      printf("X:%f\tY:%f\tPITCH:%f\tROLL:%f\tYAW:%f\tG_AutO_Num:%d\tDT35L:%f\tDT35B:%f\tWhich_Goal:%d\tStrightX:%f\tStrightY:%f\tIs_Remake:%d\tCamera_X:%f\tCamera_Y:%f\r\n",Robot_Pose.wx,Robot_Pose.wy,Robot_Pose.pitch,Robot_Pose.roll,Robot_Pose.yaw,g_Auto_Num,DT35_distance[0],DT35_distance[1],Chassis_Sense_Value.Which_Goal,Print_Stright_Pose.wx,Print_Stright_Pose.wy,Is_Remake,Camera_Return.x,Camera_Return.y);
//      printf("percent:%f\tPath_Size:%f\tPAth_Dir:%f\tPid_size:%f\tPid_dir:%f\tPid_Omega:%f\tStore_Fix:%f\tDt35_first:%f\tDT35Fix:%f\tColor:%d\tFlag_:%d,%d\r\n",path_percentNow,Print_Path_Vel.size,Print_Path_Vel.dir,Print_Pid_out.transVel.size,Print_Pid_out.transVel.dir,Print_Pid_out.omega,Chassis_Sense_Value.Store_Fix_X,Chassis_Sense_Value.DT35_Fix_First,Chassis_Sense_Value.DT35_Fix,Percept_RePack.DivideBall.colour,Chassis_Sense_Bool.Recv_Blue_Ball,Percept_RePack.DivideBall.is_send_ball);
    osDelay(3);    //TODO

  }
  /* USER CODE END Start_Printf */
}

/* USER CODE BEGIN Header_Start_Go_PAth */
/**
* @brief Function implementing the Path_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Go_PAth */
void Start_Go_PAth(void *argument)
{
  /* USER CODE BEGIN Start_Go_PAth */
    if(Is_Remake != true)
        Chassis_Sense_Bool.Off_B_Spline = true;
  /* Infinite loop */
  for(;;)
  {

          if(Go_Num_Re>6 || Chassis_Sense_Bool.Off_B_Spline == true)
              PATH_GoB_SplinePAth();


      osDelay(2);
  }
  /* USER CODE END Start_Go_PAth */
}

/* USER CODE BEGIN Header_Start_Find_Ball */
/**
* @brief Function implementing the Auto_Find_Ball_ thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Find_Ball */
void Start_Find_Ball(void *argument)
{
  /* USER CODE BEGIN Start_Find_Ball */
  /* Infinite loop */
  for(;;)
  {
      if(Is_Remake)
          Remake_Path();
      if(Go_Num_Re>6 || g_Auto_Num == 6)
          vTaskSuspend(Auto_Find_Ball_Handle);

    osDelay(2);
  }
  /* USER CODE END Start_Find_Ball */
}

/* USER CODE BEGIN Header_Start_Move */
/**
* @brief Function implementing the Robot_Move_Task thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Move */
void Start_Move(void *argument)
{
  /* USER CODE BEGIN Start_Move */
  static bool ONce = true;
  Time_ = 0;
  static bool Start_Move =false;
  /* Infinite loop */
  for(;;)
  {
      if(Time_ >= 12000)
          Start_Move = true;
      if (Start_Move || Is_Remake)
      {
          if (ONce) {
              setAirCylinder_more(Air_2, Open_Air, 8, OUT_1, OUT_2, OUT_3, OUT_4, OUT_5, OUT_6, OUT_7, OUT_8);
              ONce = false;
          }
          robotMoveTask();//TODO//TODO//TODO//TODO//TODO
      }

//      if(g_Auto_Num>=16)
//          vTaskSuspend(Robot_Move_TaskHandle);
    osDelay(3);
  }
  /* USER CODE END Start_Move */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */

